#include "luke.h"

void dvp(int verb) {
    int sockets[MAX_NODES];          /* Keep a list of sockets */
    int sources[MAX_NODES];          /* Keep source id's for each socket */
    int num_sockets;                 /* The number of sockets kept */
    int maxSock;                     /* The maximum socket number for select */
    fd_set sockSet;                  /* Set of sockets to pass to select */
    node src;                        /* The source node of the packets received */
    char *buffer;                    /* Store the received message */
    struct link *g_ls, *lk;          /* Used to iterate through links */
    int idx;                         /* Index variable */
    struct timeval selTimeout;       /* Timeout for select call */

    /* Set the select timeout */
    selTimeout.tv_sec = 0;
    selTimeout.tv_usec = 0;

    buffer = (char *) malloc(MAX_BUFFER_SIZE * sizeof(char));
    if (!buffer) {
        fprintf(stderr, "malloc() failed\n");
        exit(EXIT_FAILURE);
    }

    /* Populate set of sockets */
    g_ls = get_g_ls();
    FD_ZERO(&sockSet);
    maxSock = 0;
    num_sockets = 0;
    for(lk = g_ls->next; lk != g_ls; lk = lk->next) {
        if (lk->peer0 == get_myid()) {
            FD_SET(lk->sockfd0, &sockSet);
            sources[num_sockets] = lk->peer1;
            sockets[num_sockets++] = lk->sockfd0;
            if (maxSock < lk->sockfd0) {
                maxSock = lk->sockfd0;
            }
        } else if (lk->peer1 == get_myid()) {
            FD_SET(lk->sockfd1, &sockSet);
            sources[num_sockets] = lk->peer0;
            sockets[num_sockets++] = lk->sockfd1;
            if (maxSock < lk->sockfd1) {
                maxSock = lk->sockfd1;
            }
        } else {
            /* This should not happen */
            fprintf(stderr, "Local node not found\n");
            exit(EXIT_FAILURE);
        }
    }
    if (select (maxSock+1, &sockSet, NULL, NULL, &selTimeout) > 0) {
        for (idx = 0; idx < num_sockets; ++idx) {
            if (FD_ISSET(sockets[idx], &sockSet)) {
                if(recvfrom(sockets[idx], buffer, MAX_BUFFER_SIZE, 0, NULL, NULL) < 0) {
                    fprintf(stderr, "recvfrom() failed\n");
                    exit(EXIT_FAILURE);
                }
                src = sources[idx];
                process_packet(src, buffer);
                if(verb)
                    print_rt();
            }
        }
    }
    free(buffer);
}

void send_rt() {
    char *buffer;
    struct link *g_ls, *lk;
    int len, idx;

    struct hostent *hp;
    struct sockaddr_in node_addr;
    int sock;
    len = idx = 0;

    create_packet(&buffer, &len);
    if ( buffer[2] == 0 ) {
        return;
    }

    /* For each neighbor, send the packet */
    g_ls = get_g_ls();
    for (lk = g_ls->next; lk != g_ls; lk = lk->next) {
        if(lk->peer0 == get_myid()) {
            /* Send packet to peer1 */   
            sock = socket(AF_INET, SOCK_DGRAM, 0);
            if (sock < 0) {
                fprintf(stderr, "sock() failed\n");
                exit(EXIT_FAILURE);
            }
            node_addr.sin_family = AF_INET;
            node_addr.sin_port = htons(lk->port1); /* Send this to the port of peer1 */
            hp = gethostbyname(gethostbynode(lk->peer1));
            if (!hp) {
                fprintf(stderr, "could not obtain address from %s\n", gethostbynode(lk->peer1));
                exit(EXIT_FAILURE);
            }
            memcpy((void *)&node_addr.sin_addr, hp->h_addr_list[0], hp->h_length);
            
            if(DEBUG) {
                printf("DEBUG: Sending packet to peer %d\n\t", lk->peer1);
                for(idx = 0; idx < len; ++idx) {
                    printf("%02x ", buffer[idx]);
                }
                printf("\n");
            }

            if (sendto(sock, buffer, len, 0, (struct sockaddr *) &node_addr, sizeof(node_addr)) < 0) {
                fprintf(stderr, "sendto() failed\n");
                exit(EXIT_FAILURE);
            }
            close(sock);

        } else if(lk->peer1 == get_myid()) {
            /* Send packet to peer0 */   
            sock = socket(AF_INET, SOCK_DGRAM, 0);
            if (sock < 0) {
                fprintf(stderr, "sock() failed\n");
                exit(EXIT_FAILURE);
            }
            node_addr.sin_family = AF_INET;
            node_addr.sin_port = htons(lk->port0); /* Send this to the port of peer0 */
            hp = gethostbyname(gethostbynode(lk->peer0));
            if (!hp) {
                fprintf(stderr, "could not obtain address from %s\n", gethostbynode(lk->peer0));
                exit(EXIT_FAILURE);
            }

            if(DEBUG) {
                printf("DEBUG: Sending packet to peer %d\n\t", lk->peer0);
                for(idx = 0; idx < len; ++idx) {
                    printf("%02x ", buffer[idx]);
                }
                printf("\n");
            }

            memcpy((void *)&node_addr.sin_addr, hp->h_addr_list[0], hp->h_length);
            if (sendto(sock, buffer, len, 0, (struct sockaddr *) &node_addr, sizeof(node_addr)) < 0) {
                fprintf(stderr, "sendto() failed\n");
                exit(EXIT_FAILURE);
            }
            close(sock);

        } else {
            /* This should not happen */
            fprintf(stderr, "Local node not found\n");
            exit(EXIT_FAILURE);
        }
    }
    free(buffer);
}

/* Look through each route in the local routing table
 * and add the ones that are changed to the buffer. */
void create_packet(char **buf, int *len) {
    int pos; /* The current position in the buffer */
    int rt_changed;
    char *buffer;
    unsigned num_updates;
    struct rte *g_rt, *rt;

    /* Allocate memory for buffer. NOTE: This memory is not freed */
    buffer = (char *) malloc (MAX_BUFFER_SIZE * sizeof(char));
    if (!buffer) {
        fprintf(stderr, "malloc() failed\n");
        exit(EXIT_FAILURE);
    }
    pos = 0;
    buffer[pos++] = 0x1;    /* Type */ 
    buffer[pos++] = 0x7;    /* Version */
    pos += 2;               /* Save room for the number of assignments here */

    g_rt = get_g_rt(); 
    num_updates = 0;
    rt_changed = 0;
    for(rt = g_rt->next; rt != g_rt; rt = rt->next) {
        /* TODO: Add all routes or only the changed ones? */
        if(rt->changed == 1 || 1)
            rt_changed = 1; /* At least one route has been changed */
       /* if (rt->changed == 1) {*/
        rt->changed = 0;                            /* This route has been broadcast already */
        buffer[pos++] = rt->d;                      /* Destination */
        buffer[pos++] = (rt->c & 0xFF);             /* First byte of integer */
        buffer[pos++] = (rt->c & 0xFF00) >> 8;      /* Second byte of integer */
        buffer[pos++] = (rt->c & 0xFF0000) >> 16;   /* Third byte of integer */
        ++num_updates;
    }
    /*
    if(!rt_changed) {
        num_updates = 0; * Nothing in the routing table has changed *
    }
    */
    /* Add the number of assignments in to the array */
    buffer[2] = (num_updates & 0xFF);
    buffer[3] = (num_updates & 0xFF00) >> 8;
    *len = pos;
    *buf = buffer;
}
void process_packet(node src, char *packet) {
    int idx, cost_me_to_dst, cost_src_to_dst, cost_me_to_src, len;
    char dst;
    unsigned num_updates;
    struct rte *rt;

    cost_me_to_src = find_rte(src)->c;
    if(cost_me_to_src == INFINITY) {
        return;
    }

    if(DEBUG) {
        len = 4 + 4*(packet[2] + (packet[3] << 8));
        printf("DEBUG: Received packet from node %d\n\t", src);
        for(idx = 0; idx < len; ++idx) {
            printf("%02x ", packet[idx]);
        }
        printf("\n");
    }

    assert(packet[0] == 0x1);   /* Make sure the type is correct */
    assert(packet[1] == 0x7);   /* Make sure the version is correct */

    num_updates = packet[2] + (packet[3] << 8);
    if (num_updates == 0) {
        /* This is an invalid packet if there are no updates */
        return;
    }

    if(DEBUG) {
        printf("\tnum_updates: %d\n", num_updates);
        printf("\tdistance vector from %d: ", src);
        for(idx = 0; idx < num_updates; ++idx) {
            dst = packet[4+4*idx];     /* destination */
            cost_src_to_dst = packet[5+4*idx] + (packet[6+4*idx] << 8) + (packet[7+4*idx] << 16); 
            printf("[%d, %d] ", dst, cost_src_to_dst);
        }
        printf("\n"); 
    }

    for(idx = 0; idx < num_updates; ++idx) {
        dst = packet[4+4*idx];     /* destination */
        if (dst == get_myid()) {
            continue; /* I already know how to get to me */
        }
        cost_src_to_dst = packet[5+4*idx] + (packet[6+4*idx] << 8) + (packet[7+4*idx] << 16); 

        if(cost_src_to_dst == INFINITY) {
            /* No route to the specified destination is available from the source of the packet. Set each
             * route in the routing table that depends on this route to be infinite. */
            rt = find_rte(dst);
            if (rt == NULL) {
                continue;
            }
            if (rt->nh == src) {
                update_rte(dst, INFINITY, src);
                printf("Updated route: \tNode %d  Cost: INFINITY NextHop %d\n", dst, src);
            }
        } else {
            cost_me_to_dst = cost_me_to_src + cost_src_to_dst;
            rt = find_rte(dst);
            if(rt == NULL) {
                printf("Added route: \tNode %d  Cost %d NextHop %d\n", dst, cost_me_to_dst, src);
                add_rte(dst, cost_me_to_dst, src);
                continue;
            } else if (rt->nh == src) {
                printf("Updated route: \tNode %d  Cost %d NextHop %d\n", dst, cost_me_to_dst, src);
                update_rte(dst, cost_me_to_dst, src);

            } else if (cost_me_to_dst < rt->c || rt->c == INFINITY) {
                printf("Updated route: \tNode %d  Cost %d NextHop %d\n", dst, cost_me_to_dst, src);
                update_rte(dst, cost_me_to_dst, src);
            }
        }
    }
}



/* Update local routing table from the link set */
void update_rt_from_ls() {
    struct link *lk, *g_ls;
    struct rte *rt, *g_rt, *peer_rt;
    node me, peer;
    int prev_cost;

    me = get_myid();
    g_ls = get_g_ls();
    g_rt = get_g_rt();

    assert(g_rt->next);

    for (lk = g_ls->next; lk != g_ls; lk = lk->next) {

        if (lk->peer0 == me) {
            peer = lk->peer1;
        } else if (lk->peer1 == me) {
            peer = lk->peer0;
        } else {
            /* This should not happen */
            fprintf(stderr, "Local node is not part of this link\n");
            exit(EXIT_FAILURE);
        }

        peer_rt = find_rte(peer);
        prev_cost = peer_rt->c; 

        if (peer_rt->nh == peer || lk->c < peer_rt->c || peer_rt->c == INFINITY) {
            printf("Updated route: \tNode %d  Cost %d NextHop %d\n", peer, lk->c, peer);
            update_rte(peer, lk->c, peer);
            /* If a link is updated, for each route rt in the routing table,
             * check to see if the next hop is the node whose link was updated.
             * Set the cost of each of these routes appropriately. */
            rt = g_rt->next;
            while (rt != g_rt) {
                if (rt->nh == peer && rt->d != peer && rt->c != INFINITY) {
                    if(DEBUG) printf("rt->c + lk->c - prev_cost: %d + %d - %d = %d\n\n", rt->c, lk->c, prev_cost, rt->c + lk->c - prev_cost);
                    printf("Updated route: \tNode %d  Cost %d NextHop %d\n", rt->d, rt->c + lk->c - prev_cost, peer);
                    update_rte(rt->d, rt->c + lk->c - prev_cost, peer);
                }
                rt = rt->next;
            }
        }
    }
}
